#include "artoolkitapi.h"

#include <QDebug>
#include "OpenCV.h"

#include <opencv/cv.h>
#include <opencv/cvaux.h>
#include <opencv/highgui.h>

#include <iostream>
#include <vector>

#include <AR/ar.h>
#include <AR/video.h>
#include <AR/config.h>
#include <AR/param.h>
#include <AR/gsub_lite.h>
#include <AR/arMulti.h>

ARParam         cameraParam;
int             patternIDs[2] = {-1, -1};

int             leftnum = 0, rightnum = 0, objectnum = 2;
static double   patt_width     = 80.0;	// Per-marker, but we are using only 1 marker.
static double	patt_center[2] = {0.0, 0.0}; // Per-marker, but we are using only 1 marker.
static double	patt_trans[3][4];		// Per-marker, but we are using only 1 marker.
double          gl_para[16];
double          gl_para2[16];
double          smooth_para[2][16];
double          smooth_para2[2][16];

int             xsize = 640;
int             ysize = 480;
ARMarkerInfo*   markerInfo;
int             markerNum;
char            *config_name = "include/Data/multi/marker.dat";
char            *config_namea = "include/Data/multi/markera.dat";
char            *objectDataFilename = "include/Data/object_data_vrml";
ARMultiMarkerInfoT  *config;
ARMultiMarkerInfoT  *configa;


ARToolkitAPI::ARToolkitAPI()
{

}

void ARToolkitAPI::argConvGlpara( double para[3][4], double gl_para[16] )
{
    int     i, j;

    for( j = 0; j < 3; j++ ) {
        for( i = 0; i < 4; i++ ) {
            gl_para[i*4+j] = para[j][i];
        }
    }
    gl_para[0*4+3] = gl_para[1*4+3] = gl_para[2*4+3] = 0.0;
    gl_para[3*4+3] = 1.0;
}

void copyOver(double *source, double destination[2][16]){
    for(int i = 0; i < 16; i++){
        destination[0][i] = destination[1][i];
        destination[1][i] = source[i];
        source[i] = (destination[0][i] + destination[1][i])/2;
    }
}

void ARToolkitAPI::ARInit(int width, int height){
    int xsize = width;
    int ysize = height;

    if (arParamLoad("include/Data/camera_para.dat", 1, &cameraParam) < 0) {
        qDebug() << "Camera calibration parameters file load error";
    }
    arParamChangeSize(&cameraParam, xsize, ysize, &cameraParam);
    arInitCparam(&cameraParam);

    if ((patternIDs[0] = arLoadPatt("include/Data/patt.hiro")) < 0) {
        qDebug() << "Pattern file load error";
    }
    if ((patternIDs[1] = arLoadPatt("include/Data/patt.kanji")) < 0) {
        qDebug() << "Pattern 2 file load error";
    }
    if( (configa = arMultiReadConfigFile(config_namea)) == NULL ) {
           qDebug() << "Config file load error";
        }
    if( (config = arMultiReadConfigFile(config_name)) == NULL ) {
           qDebug() << "Config file load error";
        }

    qDebug() << "finished loading";
}

bool ARToolkitAPI::FindShit(IplImage *cvimage){

    markerInfo = NULL;
    markerNum = -1;
    double          err;



    ARUint8* image= (ARUint8 *)malloc(sizeof(ARUint8) * xsize * ysize * 4);

    for(int i = 0; i < ysize; i++) {
        for(int j = 0; j < xsize; j++) {
            image[4*i*xsize+4*j+0] = cvimage->imageData[cvimage->widthStep * i + j*cvimage->nChannels+0];
            image[4*i*xsize+4*j+1] = cvimage->imageData[cvimage->widthStep * i + j*cvimage->nChannels+1];
            image[4*i*xsize+4*j+2] = cvimage->imageData[cvimage->widthStep * i + j*cvimage->nChannels+2];
        }
    }

    if((arDetectMarker(image, 100, &markerInfo, &markerNum)) < 0){
        qDebug() << "you got nothing";
    }

    if( (err=arMultiGetTransMat(markerInfo, markerNum, config)) < 0 ) {
      //  qDebug() << "This didn;t work";
    } else {
       // qDebug("err: %d", err);
        arglCameraViewRH(config->trans,gl_para, 0.01);
        copyOver(gl_para, smooth_para);
       // qDebug("%f, %f, %f", gl_para[12], gl_para[13], gl_para[14]);
    }
    if( (err=arMultiGetTransMat(markerInfo, markerNum, configa)) < 0 ) {
       // qDebug() << "This didn;t work";
    } else {
     //   qDebug("err: %d", err);
        arglCameraViewRH(configa->trans,gl_para2, 0.01);
        copyOver(gl_para2, smooth_para2);
    }

    /* check for known patterns */
//    for(i = 0; i < 2; i++ ) { // for each object....
//        k = -1; // detected marker references
//        for( j = 0; j < markerNum; j++ ) { // for the first marker (marker 1, then 2)
//            if( patternIDs[i] == markerInfo[j].id) {  //check if object 1 == id of first marker recorded

//                arGetTransMatCont(&(markerInfo[j]), patt_trans, patt_center, patt_width, patt_trans);
//                //qDebug("Got it %d", markerNum);

//                // argConvGlpara(patt_trans, gl_para);
//                arglCameraViewRH(patt_trans,(i == 0 ? gl_para : gl_para2), 0.01);
//                if( k == -1 ) k = j;
//                else /* make sure you have the best pattern (highest confidence factor) to ensure you don't find same marker twice? */
//                    if( markerInfo[k].cf < markerInfo[j].cf ) k = j;
//            }
//        }
//    }

    free(image);
    return true;
}

double *ARToolkitAPI::GetTransformationMatrix(double *matrix, int which)
{
    if(which == 0){
        for(int i = 0; i < 16; i++){
            matrix[i] = gl_para[i];
        }
    } else if (which == 1){
        for(int i = 0; i < 16; i++){
            matrix[i] = gl_para2[i];
        }
    }
    return (which == 0 ? gl_para : gl_para2);
}

